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ABSTRACT 


The inverse and forward dynamics problems for multi-link serial manipulators are 
solved by using recursive techniques from linear filtering and smoothing theory. The 
pivotal step is to cast the system dynamics and kinematics as a two-point 
boundary-value problem. Solution of this problem leads to filtering and smoothing 
techniques identical to the equations of Kalman filtering and Bryson- Frazier fixed 
time-interval smoothing. The solutions prescribe an inward filtering recursion to 
compute a sequence of constraint moments and forces followed by an outward recursion 
to determine a corresponding sequence of angular and linear accelerations. In addition 
to providing techniques to compute joint accelerations from applied joint moments (and 
vice versa), the report provides an approach to evaluate recursively the composite 
multi-link system inertia matrix and its inverse. The report lays the foundation for the 
potential use of filtering and smoothing techniques in robot inverse and forward 
dynamics and in robot control design. 
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1 . 


INTRODUCTION 


The central theme of this report is to examine the use of filtering and smoothing 
techmques in studyiag robot dynamics. In particular, the report shows that the 
recursive difference equations of Kalman filtering [1] and Bryson- Frazier fixed 
time-interval smoothing [2], arising in the state estimation theory [3] for linear 
discrete-time state space systems, can be used to solve the problems of serial 
manipulator inverse and forward dynamics. The configuration analyzed is that of a 
joint-connected N-link serial manipulator attached to an immobile base. The joints are 
assumed to be rotational, although extension to configurations with joints allowing 
translation is simple. The inverse dynamics problem is to find the joint moments to 
achieve a set of prescribed accelerations. The forward dynamics problem is to 
determine the joint accelerations resulting from a set of applied joint moments. 
Typically, inverse dynamics solutions are useful for control design, whereas forward 
dynamics solutions are useful for system simulation. 

The solutions obtained are recursive in the sense that an inward recursion, which 
starts from the tip of the manipulator and proceeds sequentially from link to link to the 
base, is used to compute a sequence of constraint forces and moments. Similarly, an 
outward iteration from the base to the tip is used to determine a corresponding 
sequence of Unk/joint linear and angular accelerations. The recursive solutions are 
0(N) in the sense that the number of required computations only grows linearly with the 
number of links. 

The notions of spatial force, acceleration and inertia [4] are used to simplify the 
statement of these recursive equations. A spatial force acting on a link is defined here 
as a 6-dimensional vector whose first three components represent a moment and whose 
last three components represent a force. Both the moment and the force forming the 
spatial force act on the link with which the spatial force is associated. Similarly, a 
spatial acceleration is defined to be a 6-dimensional vector formed by an angular 
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acceleration and a linear acceleration. An additional concept introduced is that of 
spatial inertia. For any given linki the spatial inertia used here is a 6-by-6 matrix 
which very compactly embodies the mass and inertia properties of the link about its 
inner joint. It should be pointed out that there are minor differences between the 
definitions for spatial force, acceleration and inertia Tised here and those of [4]. 

One of the important steps in the report is to recognize that the equations of 
translational and rotational motion (derived from Newton’s second law) for each link 
can be cast as a linear difference equation that allows the spatial force at the inner 
joint to be computed from the spatial force at the outer joint and the spatial 
acceleration of the link. The difference equation is very similar to those describing the 
evolution of the state of a discrete-time state space system [1]. The spatial force plays 
the role of the state. The link spatial interval, defined as the vector from the inner to 
the outer joint of a link, plays the role of the time interval between discrete time 
samples. This establishes a means to "propagate" the spatial force inwardly within a 
link from the outer to the iimer joint. In addition, since the magnitude of the spatial 
force is continuous at the joints (due to Newton’s third law), a means also exists to 
propagate the spatial force across a joint at the interface between two adjacent links. 
Recursive use of these two propagation mechanisms allows a complete link-to-link 
sequential propagation of the spatial force from the tip of the manipulator to its base. 
The difference equation generates the joint moments as an output. The transformation 
from spatial forces to joint moments is a projection operator that takes the 
6-dimensional spatial force into the scalar that characterizes the applied joint moment 
along the joint axis. It should be stressed that the equation for the spatial forces is a 
difference equation in space and not in time. There is no time discretization involved, 
and a fully continuous time evolution is retained. 


2 


Similarly, a complementary difference equation is obtained that produces a set of 
spatial accelerations as its solution and uses the joint accelerations as inputs. The 
spatial accelerations play the role of the co-states (or adjoint variables) that are 
typical in optimal control and estimation problems [3]. This co-state equation reflects 
the kinematic relationship that exists between the spatial (angular and linear) 
acceleration at the outer joint of a link and the acceleration at its inner joint. Thus, 
the equation computes the spatial acceleration of a link at its outer joint given the 
acceleration of its inner joint. The difference equation can therefore be used to 
"propagate" in an outward direction the spatial accelerations within a link. A similar 
outward propagation across the joint at the interface between two adjacent links is 
obtained because the angular acceleration along the joint axis introduces a "jump" 
discontinuity in the angular acceleration component of the co-state (spatial 
acceleration) vector. 

When combined, the above state and co-state difference equations define a 
two-point boundary-value problem that very closely resembles those typically 
encountered as necessary (and at times sufficient) conditions for optimality in optimal 
control and estimation theory. The boundary conditions in this problem are that the 
state vanishes at the tip of the manipulator and the co-state vanishes at the base. 
These conditions arise because of the assumptions that the tip is unconstrained and the 
base is immobile (xindergoes no accelerations). This boundary-value problem defined in 
terms of state and co- state variables is \jsed as a pivotal step to develop the recursive 
inverse and forward dynamic solutions. 

Consider first the inverse dynamics problem. Its solution is obtained by means of 
a two-stage process involving: 1) an outward recursion from the base to the tip to 
obtain a set of co-states (spatial accelerations), using the set of prescribed joint 
accelerations and the boundary condition at the manipulator base*, 2) an inward 
recursion from the tip to the base using the results of the first stage above and 
producing a set of states (spatial forces) and the required applied joint moments. 
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Still within the context of inverse dynamics, it is possible to use the above 
two-stage process to obtain the by now traditional dynamical equations for an N-link 
manipulator, expressed in terms of an N-by-N composite system inertia matrix. To 
arrive at this eqxiation requires that the two difference equations of the two-point 
boundary-value problem be solved symbolically instead of numerically. In particular, 
the solution of the state equation is obtained in terms of a weighting pattern (or kernel) 
dependent on the transition matrix inherent in the state difference equation. Similarly, 
the co-state difference equation is solved in terms of the transpose of the transition 
matrix. Substitution of the solution for the co-state into that of the state leads to the 
desired form of the equations of motion. An interesting by-product of this process is a 
method for recursive computation of the inertia matrix itself by means of an inward 
iteration from the tip of the manipulator to its base. This recursive relationship for the 
inertia matrix is equivalent to those that describe the evolution of the covariance of 
the state of a linear discrete-time state space system that is driven by a white-noise 
process. With this result, the similarities between the statistical state estimation 
theory for discrete-time systems and recursive robot arm dynamics begin to reveal 
themselves. More similarities become apparent upon investigation of the forward 
dynamics problem as outlined below. 

Solution of the forward dynamics problem is also based on using the two-point 
boundary-value problem as a starting point. The key idea is to seek a solution of the 
form Xj^ = 4 where x^^ and denote the state and co- state for link k. The 

symbol denotes a 6-dimensional vector which turns out to play the role of the 
predicted state estimate whose evolution is described by the Kalman filter. The applied 
joint moments play the role of the measurement process. Similarly, is a 6-by-6 
matrix, with the units of spatial inertia, analogous to the predicted state estimation 
error covariance. This substitution, central to the “sweep method" referred to in [3], 
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leads to a two-stage computation consisting of: 1) inward filtering to obtain a sequence 
of state (spatial force) estimates and a corresponding "innovations” process defined as 
the difference between the actual and the predicted Joint moment; 2) outward 
smoothing in which the innovations process resulting from the first stage is used to 
generate a sequence of co-states (spatial accelerations) and the desired joint 
accelerations. 

The filtering recursions have a predictor/corrector architecture corresponding to 
that of the Kalman filter, specialized to the case of no measurement noise. Prediction 
occurs by means of a difference equation that for each link allows computation of a 
state estimate for the spatial force at the inner joint using the previously obtained 
state estimate at the outer joint. Correction occurs in transferring the state estimate 
across a joint between two adjacent links. In this correction step, the updated state 
estimate is obtained as a linear weighted combination of the predicted estimate and the 
innovations process. The weight associated with the innovations is a 6-by-l matrix 
playing the role of the Kalman gain. This gain can be computed from the matrix P^. 
The matrix P^ satisfies a difference equation analogous to the Riccati equation 
encountered in discrete-time systems. Propagation of this matrix from the outer to the 
inner joint of each link is achieved by using the system transition matrix. The spatial 
inertia associated with a link, playing the role of the process error covariance, appears 
as a driving term in this propagation equation. Correction occurs at each joint by 
means of the update equation of the discrete Riccati equation. 

The second stage is an outward recursion that aims to compute a sequence of 
co-states (spatial accelerations) using the innovations as an input. It also produces the 
desired joint accelerations. The computations involved in this stage are identical to 
those of the fixed-time interval smoother [2,3] of linear state estimation theory. The 
smoother is mechanized by means of what are referred to as the Bryson- Frazier 
equations. These eqxiations also have a predictor/corrector architecture. Prediction 


5 


occurs in outward propagation of the co-states within a link from the inner joint to the 
outer joint. Correction occurs in propagating the co-state from one link to the next 
across a joint. 

If the same two-stage filtering/smoothing process is conducted symbolically 
(instead of numerically), it is possible to arrive at a closed-form expression for the 
inverse of the inertia matrix. To arrive at this equation, it is first necessary to solve 
the filtering equation for the predicted state estimate in terms of the transition 
matrix associated with the Kalman filter. Similarly, the smoother equations for the 
co-states can be solved symbolically in terms of the transpose of this transition 
matrix. Substitution of the solution for the states into the solution for the co-states 
leads to the desired equation of motion. An interesting by-product of the above process 
is that it is possible to obtain recursive relationships for direct, non-numerical 
evaluation of the inertia matrix inverse. The recursive relationships are identical to 
those necessary to compute the covariance of the smoothed state estimate in a fixed 
time-interval smoother. They involve an inward recursion to compute the predicted 
state estimation error covariance, followed by an outward recursion to obtain the 
covariance of the co-states. 

The remaining sections of the report describe the configuration, the notions of 
spatial force, acceleration and inertia, recursive system dynamics and kinematics, the 
two-point boundary-value problem, inverse and forward dynamics solutions, similarities 
to the Kalman filter and the Bryson- Frazier smoother, closed form inversion of the 
inertia matrix, relationship to other work, and concluding remarks. 

2. CONFIGURATION AND PROBLEM STATEMENT 

Consider a mechanical system of N links numbered 1,...,N connected together by 
N joints numbered 1,...,N to form a branch-free kinematic chain. The system is 
illustrated in Fig. 2.1. 
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JOINT 0 



'•JOINT N-1 



The links and joints are numbered in an increasing order that goes from the tip of 

the system toward the base. Joint N is the last in the sequence, and it connects link N 

to an immobile base. Joint k in the sequence connects links k and k-f 1. Joint 0 can be 

selected at any arbitrary point in link 1. Note that link and joint numbers increase 

toward the base of the system. This differs from the more common numbering 

approach in which the numbers increase toward the tip. The ordering shown in Fig. 2.1 

allows a simpler description of the recursive algorithms contained in the report. 

Let link k be characterized by an inertia tensor Ij, about joint k, a mass nij,, a 

vector Lj. from joint k to k-1, and a vector p^^ from joint k to the link k mass center. 

Let joint k be characterized by a unit vector hj, along its axis of rotation. Let Tj, 

be the active moment applied about the axis of joint k. Let Uj^ be the corresponding 

joint angle which is positive in the right hand sense about h^. 

The objective is to outline a recursive method for computation of the joint 

and T. . A secondary objective 
k 

is to solve the closely related inverse problem of computing from the desired 

• • 

accelerations u. . 


accelerations Uj^, given the values of nij^, L^^, Pj^, h^ 
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3. 


SPATIAL FORCE, ACCELERATION AND INERTIA 


To describe simply the recursive dynamics solutions, it is convenient to define the 
notions of spatial force, acceleration and inertia. The definitions used here are closely 
related but not identical to those of [4]. Generally, the term spatial force for a given 
link k will be used here to refer to a 6-by-l vector whose first three components are 
pure moments and whose last three components are forces. Similarly, the term spatial 
acceleration will be used to describe a 6-by-l vector consisting of three angular 
acceleration and three linear acceleration components. The link k spatial inertia is a 
6-by-6 matrix that summarizes the mass and inertia properties of link k about joint k. 
A more detailed definition of these concepts is provided below. 

• T^ and f* are 3-by-l vectors representing, respectively, the constraint 

moment and force acting on link k4l at joint k. The spatial force x^ is the 
6-by-l composite vector defined by x^ = where the "+" 

superscript indicates that the corresponding variable is evaluated at a point 
on link k4l that is immediately adjacent and on the "positive" side, toward 
the base, of joint k. 

• T^ and f^ are 3-by-l vectors representing, respectively, the constraint 
moment and force acting on link k at joint k. The spatial force x^ is the 
6-by-l composite vector defined by Xj^ = [Tj^jfj^]. The "-" superscript 
indicates that the corresponding variable is evaluated at a point on link k 
that is immediately adjacent and on the "negative" side, toward the tip, of 
joint k. Note that Newton's third law implies x^ = xj^. 

• Tj^ and f^^ are, respectively, the external moment and the force (due to 
gravity, for example) acting on link k at its mass center. 
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• and are 3-by-l vectors representing* respectively, the angular and 

linear velocity oC link k4 1 at joint k. The corresponding spatial velocity is 
defined as ^ assumed that both the angular and linear 

velocities associated with a link are specified in a coordinate frame 
attached to the link. 

• and v^ are 3-by-l vectors representing, respectively, the angular and 
linear velocity of link k at joint k. The corresponding spatial velocity is 

= [o”|v^]. The superscript indicates that the corresponding variable is to 
be evaluated on the negative side of joint k. 

• and Vj^ are 3-by-l vectors representing, respectively, the angular and 
linear velocity of link k at its mass center. 

• D*R*/Dt* is a 3-by-l vector representing the linear inertial acceleration of 

link k+1 at joint k, where the operator D( *)/Dt denotes time differentiation 
in an inertial reference frame. Similarly, D ^ acceleration of 

link k at joint k, and D^R^/Dt^is the acceleration of the link k mass center. 

Subsequently, it will be convenient to express these inertial accelerations in a 
local coordinate frame attached to link k. To this end, observe that v^ = DR^/Dt, the 
linear velocity of the link k mass center, is a 3-by-l vector expressed in link k 
coordinates. Hence, 

D*Rj,/Dt“ = 

where Vj^ = dv^^/dt denotes that the time derivative of the velocity is performed in a 
coordinate frame attached to link k. Similar relationships can be obtained for the 
accelerations D*Rj^/Dt* and D*R|,/Dt*. This allows the following definition of the 
notion of spatial acceleration. 
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• The spatial acceleration ^ ^ 6-by-l vector of angular and 

■f * 4 * 4 

linear accelerations of link k at the negative side of joint k. * ^^k^^k^ 

is a vector of accelerations of link k4l on the positive side of joint k. = 

• • 

[c»ki''^k^ is the acceleration of the link k mass center. These accelerations 
are expressed in link k coordinates. 


The spatial inertia matrix for link k is defined as 



“k\ 

m^U 


(3.1) 


where is the inertia dyadic of link k about joint k; is the vector from joint k to the 
link k mass center; is the 3-by-3 matrix equivalent to the cross-product operation 
p^x; and U is the 3-by-3 identity. Note that the spatial inertia matrix summarizes the 
inertia and mass properties of link k about joint k. 

As an aside, observe that the kinetic energy T^ associated with link k can be 
e3q>ressed as 


T^ = Vz v: 


M^. 


(3.2) 


where is the 6-by-l vector of link k spatial velocities at joint k. This follows 
because 



(3.3) 
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where is the link k inertia about its mass center; is the link k angular velocity; 
and is the link k mass center linear velocity. However, 



(3.4) 


where and v^ are, respectively, the angular and linear velocities of link k at joint 
k. Finally, substitute (3.4) in (3.3) and observe that = [<d^| v^^] to obtain (3.2). 

For later reference, it is also convenient to define the following 6-by-6 matrix 




(3.5) 


where L. is the 3-by-3 matrix equivalent to L. x; and L, is the vector from 

KyTn KyTIl 

joint m to joint k. This matrix has the following properties usually associated with a 
“transition” matrix for a discrete linear state space system [1]: 


♦k,m = '*’k,i\m’ ^k,m = Vk 

which state that the matrix satisfies the semigroup property, that it becomes the 
identity when its two arguments (its subscripts) coincide, and that the matrix can be 
inverted by reversing its two arguments. 


4. DYNAMICS: AN INWARD RECURSION FOR THE JOINT SPATIAL FORCES 

The main objective of this section is to establish that the spatial forces x. satisfy 
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*k “ ‘^^k.k-l *k-l ®*k H ^ ^k’ 

k = 1,...,N 

(4.1) 

7 ^ + 
11 

k * 1,..,N 

(4.2) 

It 

o 


(4.3) 


where is the 6- by- 1 vector of joint k spatial accelerations and 
r (o^x v;) - x f J 


{ 


k 


(4.4) 


“S: "k ” ’'k - ‘k 


The arguments required to establish this result can be summarized as follows. Equation 
(4.1) is based on the rigid-body equations of rotational and translational motion for link 
k. Equation (4.2) reflects the equivalence of action/reaction moments and forces at 
joint k. Equation (4.3) states that the initial joint 0, which can be specified at any 
location on link 1, is not under the influence of any external moments and forces. A 
more detailed proof of this result is outlined below. 


Proof . Consider the equation of rotational motion for link k about its mass center 


^ • "k * “k >“k • "k * ’^k * V 1 * ■'k * •Vl'k' ” ‘k- X - 'k * 'k 


(4.5) 


where is the link k angular velocity; is the link inertia about its mass center ; 
and f^ are the moment and force acting at joint k; ^ and f^ ^ are the moment and 
force acting at joint k-1; and and f^ are the moment and force acting at the link 
mass center. 
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The translation of the link k mass center is described by 




(4.6) 


where 4 x is the inertial acceleration of the mass center. Substitution of 
this on the right side of (4.5) leads to 


V "k * "k • "k * ”k '’k ^ <»k * “k ” V " 


Tk ^ *l;.l * * Pj; X ^1; 


(4.7) 


However, recall that ^ Therefore, ^ + “k ^ ^k 

Oj, X p 4 X X Pj^), and 


Vj.=Vk + 




(4.8) 


Substitution of (4.8) in (4.7) and use of well-known cross-product expansion identities 
lead to 


\ ■ -■'i-r'-k ’"k-i * "k • “k ‘ “Vk ” * 

X (u^x y-) -T^ - x 

where I|, = Ij. + UPj.* ^ inertia about joint k. 

Now, substitute (4.8) in (4.6) to obtain 


(4.9) 
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(4.10) 


^ 


k- 1 ^ "S:"k ^ *^k " “k^k ^ “k"k ^ ^k - ^k 


Equations (4.9) and (4.10) combine into 



U L 




He 


4 





.-"Vk ”k" 



"k " "k- "k-*'Vk '"k" V - ’^k -■’k * 'k 

”S;"k’‘’'k-^ 


(4.11) 


Recall that *k_i "" ^~^k-l*"^k-l^ ^k ^ (4.11) is a 

more detailed version of (4.1). 

To establish Eq. (4.2), it is enough to observe that implies (and is implied 

by) 



which state that the moment Tj, and force f acting on link k at joint k are equal to the 
negatives of the reaction moment and force f^ acting on link k4l at joint k. 

Finally, Eq. (4.3) reflects the assumed lack of constraints (due to loads, for 
example) at the tip of the system on link 1. 
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5. KINEMATICS: AN OUTWARD RECURSION FOR THE JOINT VELOCITIES AND 
ACCELERATIONS 

The sequence of spatial velocities satisfy 

k=N,...,l (5.1) 

k=N,...,l (5.2) 

Vjj = 0 (5.3) 

where and are, respectively, the spatial velocities of link k at joints k and k-1; 

T • 

Hj, = [hj,| 0] is a l-by-6 vector based on the unit vector h^along the joint axis; and tij^is 

the relative angular velocity at joint k. 

The accelerations satisfy the closely related recursion 


K-i ■ <k-i '’i • 

V- . v‘ . 





(5.4) 



.• 

H.Uj.4 




■k’ 



(5.5) 


where tl is the "bias" acceleration 


%• 






(5.6) 


Proof ; Consider the location of the joints k and k-1 with respect to an inertial 
reference, as illustrated in Fig. 5.1. 
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Observe that 



(5.7) 


(5.8) 


where j = DR^ _^/Dt and v' = DR^/Dt are the inertial velocities of the two joints. 
In addition! the angular velocity of link k at the two joints is common, since the link is 
assumed to be rigid. Hence, 



which is a more detailed version of (5.1). 
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To establish (S.2)i observe that 






(5.10) 


(5.11) 


which state that the linear velocities on both sides of |oint k are equal to each other 
and that the relative angular velocity between links k and k4 1 equals the joint rotation 

The boundary condition (5.3) is based on the assumption that the manipulator is 
attached to an immobile base. 

The recursive relationships (5.4) - (5.6) for the accelerations can be established by 
appropriate time differentiation of (5.1) > (5.3), or of the equivalent equations (5.8), 
(5.10) and (5.11). Observe that (5.8) inq>lies 




"k >“-k ♦ “k <"k V 


However, (5.8) and ^ imply 


(5.12) 


<S13> 

This establishes (5.4). Similarly (5.10) and (5.11) imply 

’k*“k’'’'k-'i*"k’‘''k 
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Use of (5.10) and (5.11) in (5.15) leads to 


Equations (5.14) and (5.16) together imply (5.5). 

Finally, the boundary condition = 0 follows from (5.3). 


(5.16) 


6. TWO-POINT BOUNDARY- VALUE PROBLEM 

The sequences of spatial forces and spatial accelerations satisfy the 
following two-point boundary-value problem: 


^k-l = ‘**k,k-l \ 

x; = X;4H^Uk4T,k 


( 6 . 1 ) 

( 6 . 2 ) 

(6.3) 

(6.4) 

(6.5) 

( 6 . 6 ) 


where is the active moment at joint k, and u^^ are the corresponding joint 
accelerations. 
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The above is a two point boundary- value problem in the sense that the boundary 
conditions (6.6) are satisfied at the two points: the manipulator tip and its base. 

Equations (6.1) and (6.2) are inward recursive equatioiu which determine the 
sequence of spatial forces, given the spatial accelerations The term defined 
by (4.4), appears as a bias term in this equation. 

Equations (6.3) and (6.4) constitute an outward propagation and update that 
determines the sequence of spatial accelerations X^, given the joint accelerations u^ 
and the "bias" acceleration term t)^. 

Equation (6.S) can be viewed as an output equation in the sense that it relates the 
state and the output by means of the state-to- output map H^,. 

This two-point boundary-value problem is analogous to those encountered in 
optimal control and estimation theory for linear systems based on quadratic criteria 
[3]. Such problems have been investigated extensively to develop filtering and 
smoothing solutions for dynamical systems. The equivalence between (6.1) - (6.6) and 
the problems in optimal control and estimation can be outlined by means of Table 6.1. 

TABLE 6.1 

Equivalence Between Two- Point Boundary- Value 
Problems in Optimal Estimation and in 
Recursive Robot Dynamics 


ESTIMATION 


ROBOT DYNAMICS 

States 


Spatial Forces 

Co-States 


Spatial Accelerations 

Measurements 

■^k 

Active Moments 

Transition Matrix 

‘^k,k-l 

Spatial Transition Matrix 

Process Error 
Covariance 


Spatial Inertia Matrix 

Known Deterministic 
Input 

^k 

Bias Spatial Force 

State to Output Map 

«k 

Projection from State 
to Joint Axis 
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A more complete investigation of this equivalence is contained in Section 9. 

The above problem can be used to solve the following two closely related 
problems: obtain the moment sequence given knowledge of the joint accelerations 
u^; obtain the joint accelerations u^ from knowledge of the active moments These 
are referred to> respectivelyt as the inverse and forward dynamics problems and are 
solved in the following two sections. 

7. INVERSE DYNAMICS SOLUTION 

The solution to the inverse dynamics problem consists of a two-stage process of 
outward recursion based on the co-state difference equation followed by an inward 
recursion based on the state equation. 

The first stage involves an outward sequential process to determine a sequence of 
spatial accelerations. This outward recursion is based on Eqs. (6.3) and (6.4) and 
assumes that the spatial bias accelerations have been previously determined from 
the spatial and joint velocities and u^ in (5.1) - (5.3). Equation (6.4) describes an 
operation by which the spatial acceleration on the positive side of joint k, the joint 
acceleration Uj^, and the bias acceleration tIj, are combined in order to obtain the 
updated spatial acceleration at the negative side of joint k. Although this has not 
been explicitly shownt in practice, in going from one link to the next, a coordinate 
transformation is typically performed that converts the spatial accelerations into the 

coordinate frame of the next link. The updated acceleration is then operated on as in 

T + 

Eq. (6.3) by the matrix <l>j. to arrive at the spatial acceleration at joint k-l. 

These two steps, consisting of update at the joints and propagation from the inner to 

the outer joint of a link, provide a sequential process that generates the spatial 

accelerations This process is started with the terminal condition * 0. 

The second stage in the inverse dynamics solution involves an inward sequential 
process to generate the spatial forces and the required joint moments. This second 
stage is based on Equation (6.1) and (6.2). 
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Equation (6.1) involves propagation of the spatial force at the outer joint of a link 
to the inner joint by using the spatial accelerations made available by the first stage 
and the bias term Equation (6.2) eiqpresses continuity of the spatial force in going 
across a joint between two adjacent links. The process starts with the boundary 
condition = 0, indicating that the initial (and fictitious joint) is unconstrained in 
motion. The process continues inward from the tip to the base \mtil the full sequence 
of spatial forces has been generated. The active moments at the joints are 
obtained as an output of this process by means of the state-to-output transformation 

«k- 

The boundary-value problem (6.1) - (6.6) can also be used to arrive at the 
traditional second-order matrix equation 


M(u) u 4 V(u,u) = X 


(7.1) 


where M(u) is the inertia matrix, and V(u,u) is a term that comprises nonlinear velocity 
and gravity dependent effects. To this end, observe that the co-state equations (6.3) 
and (6.4) imply 


N 




*T T • 

<t>. . (H. U. 4 Tl.) 
^1,) 1 1 'l 


(7.2) 


i=j 


Similarly, the state equations (6.1) and (6.2) imply 
k 

j=l 


(7.3) 


Substitution of (7.2) in (7.3) leads to 


k N k 

j*l i=j j=l 


(7.4) 
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Howevefi observe the identity 
k N N min(i,k) 

EE‘E E 

j=l i=j i=l j=l 

which can be established by inspection of Fig. 7.1 



Fig. 7.1 Illustration of Double Summation Order Reversal 


Use of this in (7.4) implies 


' E ^.i * V * E *k.i f i 


where r . is the 6-by-6 matrix 
min(i,k) 

r. . = <|>. .M. <b^. 

k,i ^k,j ) ^1.) 

j=l 


Note that r^ ^ can be specified as 


l^i^k^N NSi^kSl 


’^k.k ‘•^i.k 








<7.8) 


k 

1^1. w = <l>t. • M. <|>J . 

k,k ^ ^k,j ) ^k,j 

j=l 

Observe also that r. . satisfies the recursive relationship 


^k+l,k+l ■ ^k4l,k ’^k.k ‘*’k+l,k ^ 


"S:4l 


(7.9) 


This formula, in filtering and prediction theory, is that satisfied by the covariance of 
the state of a linear discrete-time system subject to a process error with covariance 


Use Xj, in (7.6) to obtain 

N 

^ nij, j + vNu,u) = Tj^, k = 1,...,N 

i=l 


where 


N 


i=l i=l 


(7.10) 


(7.11) 


This is the scalar version of the matrix equation (7.1), where V = [V^,..., v“) ; M is the 
N-by-N inertia matrix whose general element is specified by (7.11); and 

T = [Tj^,...,Tjj] is the vector of aU active joint moments. 


8. FORWARD DYNAMICS SOLUTION 

The forward dynamics problem is to find the accelerations Uj, at the joints, given 
the active moments Xj^. This problem can be solved using (6.1) - (6.6) and what is 
referred to as the "sweep method" in [3]. The sweep method begins by assuming that 
the state Xj^ and the co-state are related by 

( 8 . 1 ) 
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where and are to be determined by means of recursive formulas that emerge upon 
substitution of (8.1) in equations (6.1) - (6.6). In these formulas, will play the role 
that the predicted state estimate plays in the Kalman filter, and will play the role of 
the corresponding state estimation error covariance. 


RESULT 8.1 The joint accelerations u^ can be computed from the applied moments 
by means of the following two-stage process of inward filtering and outward smoothing. 
Filtering 


Initial Condition 

zl-0 

(8.2) 

State 

Prediction 


(8.3) 

Spatial hiertia 
Prediction 

^k " ‘•*k,k-l ^k-1 ^k,k-l ^ 

(8.4) 

Gain 

Conqnxtation 


(8.5) 

Innovations 


(8.6) 

State 

Update 

II 

(8.7) 

Residuals 


(8.8) 

Spatial boertia 


(8.9) 


Update 

The residuals e^ and the gains are stored in this stage. 

The scalar whose inversion is required to compute the gain G^,, represents the 
inertia along the joint k axis of the composite body formed by links l,...,k. 
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Smoothirm 


Terminal Condition = 0 

Co-St.t. X- 

Propagation 


( 8 . 10 ) 

( 8 . 11 ) 


Joint Accelerations ^ ^ (8.12) 

Co-State Update “ ^k \ k “ k“ ^ V ^ k^ * ^ k®k 

Not shown explicitly is a link k to link k-i- 1 coordinate transformation that is performed 
immediately after a joint has been crossed and the state and spatial inertias have been 
updated in (8.7) and (8.9). A similar transformation from link k-fl to link k is performed 
after the co-state update in (8.13). 


Proof: Substitute (8.1) in (6.5) to obtain 


®k “ ^^kH 


(8.14) 


where e^ is the innovations process defined by 



(8.15) 


NoWi substitute (6.4) in (8.14) to obtain 




- T 

where Hence, 

•i^^.e;-oJ(X;.„^) (8.U) 
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T ^-1 


where is the Kalman gain G^^ = Pj^H^D ^ . Substitute this in (6.4) to obtain 




(8.18) 


which verifies (8.13). Substitute (8.1) in (6.1) to obtain 


V^k^k “ ‘^^k.k-lVl ^k,k-l ’^k-l^k-l * ^^k ^k 


(8.19) 


However, in view of (6.3), 


+ .T 


\ *"k\ " ‘*’k,k-lVl * ‘*^k,k-l \-l ‘*’k,k-l\ ^k ^k 


( 8 . 20 ) 


The state and spatial inertia propagation equations (8.3) and (8.4) are sufficient 
conditions to satisfy (8.20). 

To obtain the state and inertia update equations, use (8.1) in the identity (6.2): 




( 8 . 21 ) 


However, substitute (8.18) on the right side of (8.21) to obtain 


< * KK ■ ’“i * •'i n-GA'”^ * V ♦ k 


and 


( 8 . 22 ) 




(8.23) 




(8.24) 
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These are the inertia and state update equations. 

As an aside, note that the updated spatial inertia satisfies the following 
alternative formulas: 

= P'd-Gj^Hj,)^ = d-Gj.Hj.) P^d-Gj,Hj,)T (8.25) 

as is well-known in Kalman filtering [1,3]. These two equations can be obtained 
routinely from (8.23). 

9. SIMILARITIES TO KALMAN FILTER AND BRYSON-FRAZIER SMOOTHER 

The two-point boundary-value problem of Section 6 and the filtering and 
smoothing equations of Section 8 are analogous to those typically used to obtain the 
best smoothed state estimate of a discrete-time state space system with discrete 
measurements (for the special case of no measurement noise). To examine this analogy 
more closely, consider the following system: 

■'k ' “k’H: 

where is the state; is the observation; ^ is the transition matrix; is the 
state to measurement map; and w^ is a white Gaussian process with mean and 
covariance specified by 

E(Wj,) = Ej,; E{<Wj,-Ej,) \ (9.3) 

where E^^ is the link k bias spatial force, and M^ is the link k spatial inertia. To 
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simplify the discussion, it is assumed in this section that the acceleration bias term tIj, 


has been set to zero. 

The filtering problem consists of finding 


z-=E(Xj./Ti,...,Tj,_i) (9.4) 

the best estimate of the state given all of the previous measurements 
Closely related to this filtered estimate is the innovations process defined by 


and the filtered state estimation error covariance 


(9.5) 


which is known to satisfy the discrete-time Riccati equation. The covariance of the 
innovations is also known to be 


T 

Note that (9.7) is obtained from the more general formiila + R j. 

setting the measurement noise covariance Rj^ to zero. The equations for the Kalman 
gain and the updated covariance are 




(9.9) 


The updated state estimation error covariance can be shovm to be 

E[(Xj^-z^) (9.10) 

where 

< ' H * Vk <’•“> 

is the updated state estimate 


z* = V 

The smoothing problem associated with (9.1) and (9.2) is to find 
^ = E(Xj,/Tj^,....Tjj) (9.13) 


the best estimate of the state, given all of the data at the N measurement 

locations. It is known [3] that the best smoothed estimate can be generated by means 
of the Bryson- Frazier eqxiations 


where are the co-states specified by 

H “ **^k+l,k H+1 


(9.14) 


(9.15) 
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This is a backward reciirsion consisting of propagation in (9.20) followed by an update in 
(9.21). The boTindary condition A.^ = 0 is valid at the sample. 

It is possible to obtain the closed- form inverse of the inertia matrix in terms of a 
pair of matrices analogom to and above. This is done in the next section. 
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10. CLOSED FORM INERTIA MATRIX INVERSE 

The central objective is to obtain the following equation: 


.. 12 3 


( 10 . 1 ) 


12 3 

where a^, and are the joint angle accelerations due to the applied moments 
the bias spatial forces and the bias spatial accelerations The three 

acceleration components are given by 


k-1 


N 


®k “ ^k'^k * *^k S Vi ^k d^T , 


U1 


Uk+1 


k-1 N 

t|Kk",r)f ^ S i 

i=l i=k4l 


( 10 . 2 ) 


k-1 


N 


i + G*^ tIrV.k*) 

i=l i=k+l 


where c^^ and dj^ are, respectively, the scalar and the l-by-6 vector 


.-I ^T.+ 


T.4 


\ = ^k ^ '"k'^k'^k’‘^k=^k'^k- \\ 


(10.3) 


and tp(k",i") = i|Kk",i*)(I-GjHj) and t|Ki^,k*) = (I-G.H.) x(r(i ,k‘^), with i|f(i .k'^) 

defined in (10.6) below. 

Recall that (7.1) implies that u = M (u)Ix-V(u,u)], where M is the inertia matrix. 
Hence, the elements of its inverse can be obtained by inspection of in (10.2). 
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The overall approach used to arrive at (10.1) is based on solving both the state and 
co-state difference equations in terms of their corresponding “weighting" kernels. 
Substitution of the co-state solution into the state solution leads to the desired results. 
This is now performed in detail. 


Solution of the State Equation 

The aim here is to show that the sequences of “predicted" spatial forces and 
residuals e^ are specified by: 

k-1 

z” = ^ t|Kk ,i*)Cj (10.4) 

i=l 

k-1 

i=l 

where i|;(k ,i ) and <^re defined as 




( 10 . 6 ) 


f. = (I-G.H.) 5. + G.t. 4 Pfq. 
111 11 11 


(10.7) 


Note that is the transition matrix for the Kalman filter. This matrix is 

evaluated at its two arguments k and i^, representing, respectively, the negative 
(outboardf toward the tip) side of joint k and the positive (inboard, toward the base) side 
of joint i. 


Proof; Recall that 


% " * ♦k.k-i Vi 


^k - «-0k“k>s - Vk * *'k'\ 


( 10 . 8 ) 


(10.9) 
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Substitution of (10.8) in (10.9) leads to 


2^ = t|r(k*,k-l^) 


( 10 . 10 ) 


where 


'*'(k^’k-l^ = (I-Gj,Hj,)4»j,j,_j 


(10.11) 


This implies that 
k-1 

*k-l “ ^ ^i (10.12) 

i=l 

where 


T|Kk-l'^,i'^) = »|r(k-l*,k-2‘^) ... ^ (i+l"^.!*) q0.13 

To obtain (10.4), substitute (10.12) in (10.8) and observe that <Kk”.i^) = 4> 

+ .+ 

t|r(k-l ,i ). This and (8.8) imply (10.5). 

Solution of the Co-State Equation 

The aim here is to show that the sequence of spatial accelerations is 
specified by 

^k 2 i|r^(i~,k'^)^ 
i=k+l 

where 

?r. = (I-GiHi)^T,. + H^e* 


(10.14) 


(10.15) 
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To establish (10.14), begin by observing that 


X- . a-G^H^)TaJ . V * 

\ ‘ Vl.k H.l 


(10.16) 

(10.17) 


with the terminal condition = 0. Substitution of (10.17) in (10.16) leads to 


\~ = ijr (k+l ,k ) 4 Tij, 


(10.18) 


where 


^(k+l"’^')=^k4l,k(I-^kHk) 


(10.19) 


Hence, 



N 

= ^tir^(r,k4r)Tr. 

i=k4l 


( 10 . 20 ) 


where 


T|»(i ,k4l ) ^I-^i_iHi_l^-‘^k42,k4l^^"^k4l ^4l^ 


( 10 . 21 ) 


T - 4 T 

To obtain (10.14), substitute (10.20) m (10.17) and observe that tjr (i ,k ) = j. 

\(f'^(i ,k4l ). 

The joint accelerations u^, can be obtained by substitution of (10.14) in (8.12): 


N 


, 4 ^T . ,n,TV^ .T,.-.4.~ 


i=k4l 


( 10 . 22 ) 
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Joint Angle Accelerations due to Joint Moments. Bias Spatial Forces, and Bias Spatial 
Accelerations 

The objective here is to obtain (10.1), To this end, observe that (10.5) and (10.22) imply 
that 


k-1 N 

i=l j=k+l 

where is defined as 

- “k’'k\ 

However, 

N N 

^r|r^(f,k‘^)T|. = [t|T^(i'^,k*)7i. + \|r^(f,k'^)H^ej ] (10.25) 

i=k+ 1 i=k+ 1 


Tjr^(i ,k'^)Tr.] 


(10.23) 


.-1 


"k = ^^k ^V«k 


'*^(k"'i')?'i -Vk 


where T|r^(i''’,k''’) = 


lr^(i“,k*) (I-G.H.)’^. 


In view of (10.5), 


N N i-1 

X ^I^V,k^)Hj^e^! = ^ T|rV,k^) h’FdT^ HAKi'o*)?^] (10.26) 

i=k+l i=k+l j=l 

Recall the identity 


N i-1 N-1 N-1 

L r ■ E E 

Uk+1 j=l j=l i=max(k+l,j+l) 

to obtain 

N i-1 

Z) ^V,k^H^D:^H.tJr(r,jM'. = 

i=k+l j=l 


(10.27) 
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(10.28) 


Ay 4 A*|. 

j=k4l HI 

where 

N 

2^t|f'^(r,k^) H?-D^^Hjt|f(r,k*) (10.29) 

i=k+l 

In arriving at the upper limit N of summation for the last term in (10.28), use has 
been made of the terminal condition = 0 implied by (10.29). Finally, use of (10.28) 
in (10.26), (10.26) in (10.25), and (10.25) in (10.23) leads to (10.1). 

Observe that (10.29) implies that the sequence satisfies the recursive 
equations 


^k “ ‘*’k+l,k \+l ^k+l,k’ 


(10.30) 




(10.31) 


These eq\iations are identical to the ones satisfied by the co-state variable covariance 
of the fixed-time smoother in Section 9. 


11. RELATIONSHIP TO OTHER WORK 

The basic reference on filtering is, of course, Kalman's original report |1], which 
derives the filter for discrete-time systems with discrete data and which, in addition, 
introduces a global framework (Riccati equation, Kalman gain, prediction/correction, 
covariances, etc.) that underlies much of today’s linear filtering and prediction theory. 
A similarly basic reference for smoothing is [2]. Reference [3] provides a summary 
exposition of both filtering and smoothing, as well as the sweep method for solution of 
two-point boundary- value problems. The main contribution of the present report is to 
recognize that these filtering and smoothing techniques provide a unified framework to 
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solve recursively the fundamental robotics problems of inverse and forward dynamics. 
This complements many of the recursive and nonrecursive techniques currently used to 
solve these problems [4-10]. 

Because they address forward dynamics (instead of the more common problems of 
inverse dynamics), Refs. [4,5] are very close in spirit to the present report. In fact, the 
recursive equations of [4] are very similar to the filtering and smoothing solutions of 
Section 8. The solutions advanced here expand on the results of [4] in two 
areas: 1) recognizing similarities with filtering and smoothing, and 2) providing what is 
believed to be a more appropriate way to account for the bias spatial forces and 
accelerations due to coriolis, centrifugal, gyroscopic and gravitational effects. 
Reference [4] suggests that these effects be accounted for by conducting an inverse 
dynamics computation prior to the forward dynamics solution. This has the possible 
drawback of requiring that certain calculations (link-to-link coordinate 

transformations, spatial force and acceleration propagation, etc.) be performed twice: 
once for inverse dyiumics and again for the forward dynamics problem. Hence, two full 
recursions along the entire span of the manipulator appear to be required. In contrast, 
the rectirsive techniques advanced here embody these effects in the bias terms and 
of the filter and smoother equations. No advance inverse dynamics solution is 
required, and a single inward/outward iteration is sufficient to solve the problem. An 
additional contribution of the present report is to introduce a framework that, in 
addition to solving the forward dynamics problem of [4], also provides inverse dynamics 
solutions. 

Another result which is believed to be unique is the closed- form evaluation of the 
inertia matrix and its Inverse in terms of estimation error covariances. This result 
suggests that numerical inertia matrix inversion can be avoided (or at least performed 
recursively). This can be done if the emphasis is placed instead on direct 
matrix- symbolic evaluation of the inertia matrix inverse (as in Section 10 of this report) 
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or in the filtering and smoothing formulas which provide a constructive procedure for 
determining Joint accelerations from applied moments. 

Many of the references [6-9} presenting recursive solutions focus primarily on the 
inverse dynamics problem. These recursive methods lead either to the evaluation of 
required Joint applied moments from desired Joint accelerations or to evaluation of an 
inertia matrix for an equation of the form (7.1). The forward dynamics problem is not 
addressed directly. Instead, the usual approach requires a numerical inversion of the 

g 

inertia matrix. This causes the resulting forward dynamics algorithms to be 0(N )• i.e., 
the number of computations is proportional to the cube of the number of links. This 
means that for large N the 'computations required may be dominated by the matrix 
inversion process. 

Yet another point of view with regards to robot dynamics is that initiated by 1 10], 
which advances the notion that explicit scalar eqtiations of motion can be obtained for 
common manipulators such as the J PL/Stanford and PUMA arms. These equations are 
eiq>licit in the sense that the scalar elements of the inertia matrix (as well as other 
matrices accounting for corioUs, centrifugal, and other effects) are evaluated 
symbolically in terms of link mass and inertia, mass center offsets, etc. The end results 
of this approach are algebraic egressions [11,12] for each of the inertia matrix 
elements. 

Such explicit equations can lead to substantial computational savings. One key 
reason for this is that terms in the inertia matrix which do not depend on the 
instantaneous value of the Joint angles (reflecting the manipulator configuration) can be 
grouped together and need be evaluated only once at the beginning of the model 
application. The same value of those terms is then retained after this initialization. 
This is a feature that less explicit equations do not have. However, because of the 
complexity of the trigonometric and algebraic operations required, manual derivation 
methods cannot be used easily, and symbolic manipulation programs [11,12] that 
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conduct machine differentiation of the Lagrangian are typically used. One of the 

challenges that remains after symbolic evaluation of the inertia matrix elements is the 

numerical inertia matrix inversion required to solve the forward dynamics problem. 

The recursive equations developed in Section 10 cam in principle* be used to 

arrive at direct explicit evaluation of the scalar elements of the inertia matrix inverse. 

A symbolic manipulation program could be set up to conduct the operations in Eq. (10.1) 

symbolically, as opposed to numerically. The end result would be a set of equations of 

12 3 

the form (10.1) where the accelerations a^, and would be determined as 
explicit functions of the )oint angles, the link masses and inertias, the link dimensions, 
etc. Such results would eliminate the need to invert the inertia matrix numerically, and 
could lead to significant computational savings. Savings comparable to those achieved 
in [11,12] for explicit evaluation of the inertia matrix could be achieved for a similarly 
explicit evaluation of its inverse. 

12. CONCLUDING REMARKS AND FUTURE DIRECTIONS 

The pivotal step presented in this report is that the dynamics and kinematics of an 
N-link serial manipulator can be described by a two-point (in space) boundary-value 
problem. This observation allows the solution of inverse and forward dynamics within a 
single unified framework based on recursive techniques from the theory of optimal 
filtering and smoothing. 

The results of this report suggest several areas for future research. 

• Development of methods for symbolic evaluation of the scalar elements in 
the inverse of the inertia matrix, as opposed to the current ones that focus on 
the elements of the inertia matrix itself. This would simplify system 
simulation as well as implementation of increasingly important exact 
linearization techniques for control design [13, 14]. 
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• Extensive numerical studies with the proposed methods to establish the same 
level of confidence as exists for current methods. 

• Development of forward and inverse dynamics solutions based on "fast" 
filtering and smoothing techniques which involve direct propagation of the 
filter gain as opposed to indirect methods requiring covariance propagation. 

• Use of the forward dynamics solutions presented here for control design. 

A full investigation of these areas will require much work and will be quite 
interesting. 
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versa) , the report provides an approach to evaluate recursively the composite multi- 
link system inertia matrix and its inverse. The report lays the foundation for the 
potential use of filtering ad smoothing techniques in robot inverse and forward dynam- 
ics and in robot control design. 
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